#include "simulation/world.h"

#include <ros/package.h>
#include <ros/callback_queue.h>
#include <visualization_msgs/Marker.h>

#include <string>
#include <stdlib.h>
#include "math.h"

World::World(int argc, char **argv) :
	robot(0.0f, 0.0f, 0.0f)
{
	std::string path = ros::package::getPath("simulation");
	path.append("/res/map2.raw");

	map = new Map(path.c_str(), 200, 200);

	ros::NodeHandle nh;
	ros::ServiceServer service = nh.advertiseService("/simulation/world/distance", &World::distance_request, this);

	motor_movement_subscriber = nh.subscribe("/motor_controller/movement", 100, &World::movement_message, this);

	if (!nh.getParam("/motor/wheel_base", wheel_base)) {
		ROS_ERROR("Tracker failed to get param /motor/wheel_base");
	}

	// Start an asyncspinner that will spin in the background and respond to callbacks
	ros::AsyncSpinner spinner(1);
	spinner.start();

	ros::Rate loop_rate(30);
	while (ros::ok()) {
		map->render();

		loop_rate.sleep();
	}
}

World::~World()
{
	delete map;
}

void World::movement_message(const driver::Movement::ConstPtr& msg)
{
	float dist_center = (msg->dist_left + msg->dist_right) / 2.0f;
	float delta_theta = -(msg->dist_right - msg->dist_left) / wheel_base;
	float delta_x = dist_center * cosf(robot.getAngle() + delta_theta);
	float delta_y = dist_center * sinf(robot.getAngle() + delta_theta);

	robot.setX(robot.getX() + delta_x);
	robot.setY(robot.getY() + delta_y);
	robot.setAngle(robot.getAngle() + delta_theta);
}

/**
 * Responds to a distance request by finding out the distance to the nearest
 * wall in the direction from the point specified in the request.
 */
bool World::distance_request(simulation::Distance::Request &request,
							simulation::Distance::Response &response)
{
	// A request is given in the coordinate system of the robot,
	// we first need to translate to world coordinates.

	// Local coordinates
	float lx = request.position_x;
	float ly = request.position_y;
	float ld = request.direction;

	// World coordinates
	float wx, wy, wd;
	wx = robot.getX() + lx * cosf(robot.getAngle()) - ly * sinf(robot.getAngle());
	wy = robot.getY() + lx * sinf(robot.getAngle()) + ly * cos(robot.getAngle());
	wd = ld + robot.getAngle();

	float distance = map->distance(wx, wy, wd, request.max_range);

	// TODO: Add the distance noise here.

	response.distance = distance;

	return true;
}

int main (int argc, char **argv)
{
	ros::init(argc, argv, "world");

	new World(argc, argv);

	return 0;
}
